#!/usr/bin/python
import numpy as np
import os
import matplotlib.pyplot as plt
DNA_SIZE = 10
POP_SIZE = 20
X_BOUND = [0, 100]
Y_BOUND = [0, 100]
Z_BOUND = [0, 100]
def translateDNA(pop):
    o_pop = pop[:, :1]#old_navfn_behavior
    u1_pop = pop[:, 1:2]#use_quadratic
    u2_pop = pop[:, 2:3]#use_dijkstra
    u3_pop = pop[:, 3:4]#use_grid_path
    a_pop = pop[:, 4:5]#allow_unknown
    x_pop = pop[:, 5:15]#planner_window_x
    y_pop = pop[:, 15:25]#planner_window_y
    z_pop = pop[:, 25:35]# default_tolerance
    p_pop = pop[:, 35:]#publish_scale
    # pop:(POP_SIZE,DNA_SIZE)*(DNA_SIZE,1) --> (POP_SIZE,1)
    o = o_pop.dot(2 ** np.arange(1)[::-1])
    u1 = u1_pop.dot(2 ** np.arange(1)[::-1])
    u2 = u2_pop.dot(2 ** np.arange(1)[::-1])
    u3 = u3_pop.dot(2 ** np.arange(1)[::-1])
    a = a_pop.dot(2 ** np.arange(1)[::-1])
    x = x_pop.dot(2 ** np.arange(DNA_SIZE)[::-1]) / float(2 ** DNA_SIZE - 1) * (X_BOUND[1] - X_BOUND[0]) + X_BOUND[0]
    y = y_pop.dot(2 ** np.arange(DNA_SIZE)[::-1]) / float(2 ** DNA_SIZE - 1) * (Y_BOUND[1] - Y_BOUND[0]) + Y_BOUND[0]
    z = z_pop.dot(2 ** np.arange(DNA_SIZE)[::-1]) / float(2 ** DNA_SIZE - 1) * (Z_BOUND[1] - Z_BOUND[0]) + Z_BOUND[0]
    p = p_pop.dot(2 ** np.arange(6)[::-1])
    return o, u1, u2, u3,a, x, y, z, p

def write_yaml(pop,curpath):
    o, u1, u2, u3, a, x, y, z, p = translateDNA(pop)
    for i in range(POP_SIZE):
        yamlpath = os.path.join(curpath, str(i) + '.yaml')
        with open(yamlpath, "w") as f:
            f.write('GlobalPlanner:\n')
            f.write(' old_navfn_behavior'+': '+str(bool(o[i]))+'\n')
            f.write(' use_quadratic'+': '+str(bool(u1[i]))+'\n')
            f.write(' use_dijkstra'+': '+str(bool(u2[i]))+'\n')
            f.write(' use_grid_path'+': '+str(bool(u3[i]))+'\n')
            f.write(' allow_unknown'+': '+str(bool(a[i]))+'\n')
            f.write(' planner_window_x'+': '+str(x[i])+'\n')
            f.write(' planner_window_y'+': '+str(y[i])+'\n')
            f.write(' default_tolerance'+': '+str(z[i])+'\n')
            f.write(' publish_scale'+': '+str(p[i]))

def write_binary(pop,path):
    write_path = os.path.join(path,'pop.txt')
    with open(write_path, "w") as f:
        for i in range(POP_SIZE): 
            p=pop[i]
            for j in range(len(p)-1):
                f.write(str(p[j])+' ')
            f.write(str(p[len(p)-1])+'\n')

def read_binary(path):
    pop=[]
    write_path = os.path.join(path,'pop.txt')
    with open(write_path, "r") as f:
        lines=f.readlines()
	for line in lines:
            p=[]
            sp=line.split(' ')
            for s in sp:
                p.append(int(s))
            pop.append(p)
    return pop
if __name__ == "__main__":
    pop = np.random.randint(2, size=(POP_SIZE, DNA_SIZE * 3 + 11))
    #o, u1, u2, u3, a, x, y, z, p = translateDNA(pop)
    #print(o, u1, u2, u3, a, x, y, z, p)
    #print("POP %s" %pop)
    gen_path = '/home/nero/yaml_gen2'
    pop_path = '/home/nero/gen_pop'
    write_binary(pop,pop_path)
    write_yaml(pop,gen_path)
    #pop_read=read_binary(pop_path)
    #print(pop_read)
    #o, u1, u2, u3, a, x, y, z, p = translateDNA(pop)
    #print(o, u1, u2, u3, a, x, y, z, p)
